/**
 * @file driver
 * @brief This file contains all the functions prototypes for the driver
 *        firmware library.
 * @author He Shaohua (hsh15529870639@foxmail.com)
 * @date 2024/11/13
 * @version 1.0
 * @copyright Copyright (c) 2024, SNUT-Chaoyue Team
 * ****************************************************************************************************
 * @par Change Log:
 * <table>
 * <tr><th>Date         <th>Version       <th>Author      <th>Description
 * <tr><td>2024/11/13      <td> 1.0          <td>He Shaohua  <td>Creator
 *
 * </table>
 * ==================================----How to use this driver----=====================================
 * [..]
 * @note
 *   -#
 *
 * @warning
 *   -#
 *
 * *****************************************************************************************************
 * @attention
 *
 * This document is required and please understand any bugs that may occur in
 * the document. If you want to modify this file, after the code update is
 * completed, update the version number in time, indicate the update date, and
 * fill in the changes Make sure that the submitted code does not have many
 * bugs.
 *
 * All rights reserved.
 * *****************************************************************************************************
 */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef MOTOR_DRIVER_USER_INC_DRIVER_H_
#define MOTOR_DRIVER_USER_INC_DRIVER_H_
/* Includes ------------------------------------------------------------------*/
#include "utils.h"
/* Private macros ------------------------------------------------------------*/
/* Private type --------------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
/* Exported variables --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
namespace Driver_DC_Motor
{
    ///
    /// 适用于单板的的直流电机驱动器，目前支持的芯片为DRV8701E
    /// https://www.ti.com.cn/product/cn/DRV8701
    /// 支持霍尔编码器速度检测
    class DC_Motor_Control_F1
    {
    public:
        ///  开环直流电机控制
        /// @param break_port 刹车控制端口
        /// @param break_pin 刹车控制IO
        /// @param phase_port 方向控制端口
        /// @param phase_pin 方向控制IO
        /// @param htim 电机控制定时器句柄
        /// @param tim_channel 电机控制定时器通道
        /// @note 如果只使用开环模式，则初始化此函数即可，但是角度，速度测量等类成员不可以调用
        explicit DC_Motor_Control_F1(GPIO_TypeDef* break_port, const uint16_t break_pin, GPIO_TypeDef* phase_port,
                                     const uint16_t phase_pin, TIM_HandleTypeDef* htim, const uint32_t tim_channel) :
            Break_Port(break_port), Break_Pin(break_pin), Phase_Port(phase_port), Phase_Pin(phase_pin), _htim(htim),
            tim_Channel(tim_channel)
        {
            Set_Motor_Break(false);
            HAL_TIM_PWM_Start(_htim, tim_channel);
        };

        /// 闭环直流电机控制
        /// @param break_port 刹车控制端口
        /// @param break_pin 刹车控制IO
        /// @param phase_port 方向控制端口
        /// @param phase_pin 方向控制IO
        /// @param htim 电机控制定时器句柄
        /// @param tim_channel 电机控制定时器通道
        /// @param htim_coder 编码器定时器句柄
        /// @param one_circle_paulse_by_motor 单圈脉冲
        /// @note 目前测速方式为编码器测速，使用此函数初始化后可调用速度角度测量
        explicit DC_Motor_Control_F1(GPIO_TypeDef* break_port, const uint16_t break_pin, GPIO_TypeDef* phase_port,
                                     const uint16_t phase_pin, TIM_HandleTypeDef* htim, const uint32_t tim_channel,
                                     TIM_HandleTypeDef* htim_coder, float one_circle_paulse_by_motor) :
            Break_Port(break_port), Break_Pin(break_pin), Phase_Port(phase_port), Phase_Pin(phase_pin), _htim(htim),
            tim_Channel(tim_channel), _htim_coder(htim_coder)
        {
            HAL_TIM_PWM_Start(_htim, tim_channel);
            Set_Motor_Break(false);
            one_circle_pause = one_circle_paulse_by_motor * 4;
            HAL_TIM_Encoder_Start(_htim_coder, TIM_CHANNEL_ALL); // 开启编码器模式
            HAL_TIM_Base_Start_IT(_htim_coder); // 开启编码器的中断
        };
        ~DC_Motor_Control_F1() = default;

        /**
         * @brief 电机开环速度设置
         * @param num 支持正反转，最大-990 --- +990
         * @note 注意正反转的方向与速度方向一致
         */
        void Set_Motor(const int16_t num) const
        {
            if (num > 0)
            {
                HAL_GPIO_WritePin(Phase_Port, Phase_Pin, GPIO_PIN_RESET);
                __HAL_TIM_SetCompare(_htim, tim_Channel, static_cast<uint16_t>(my_Abs<int16_t>(num)));
            }
            else if (num < 0)
            {
                HAL_GPIO_WritePin(Phase_Port, Phase_Pin, GPIO_PIN_SET);
                __HAL_TIM_SetCompare(_htim, tim_Channel, static_cast<uint16_t>(my_Abs<int16_t>(num)));
            }
            else if (num == 0)
            {
                HAL_GPIO_WritePin(Phase_Port, Phase_Pin, GPIO_PIN_SET);
                __HAL_TIM_SetCompare(_htim, tim_Channel, static_cast<uint16_t>(my_Abs<int16_t>(num)));
            }
        }

        /**
         * 电机刹车控制
         * @param set 设置为true驱动器将失能
         */
        void Set_Motor_Break(const bool set) const
        {
            if (set)
                HAL_GPIO_WritePin(Break_Port, Break_Pin, GPIO_PIN_RESET);
            else
                HAL_GPIO_WritePin(Break_Port, Break_Pin, GPIO_PIN_SET);
        }

        /**
         * 电机速度、角度更新，应在100Hz频率下运行
         * @attention
         */
        void Set_Angle_Rate()
        {
            int16_t temp = 0;
            temp = __HAL_TIM_GetCounter(_htim_coder);
            __HAL_TIM_SetCounter(_htim_coder, 0);
            last_coder += temp;

            motor_position_angle += temp; // 位置环参数记录

            // 角度测量
            if (last_coder > static_cast<int16_t>(one_circle_pause))
                last_coder = 0;
            else if (last_coder < 0)
                last_coder = static_cast<int16_t>(one_circle_pause);

            // 标幺，乘比
            motor_angle_out = static_cast<float>(last_coder) / one_circle_pause * 4096.0f;

            speed_times++;
            if (speed_times > 19) {
                // T法
                motor_speed = motor_position_angle / one_circle_pause * 50.0f;
                motor_position_angle = 0;
                speed_times = 0;
            }
        }

        /**
         * 获取电机当前角度
         * @return 0~4096为电机一圈的角度
         */
        [[nodiscard]] float get_angle() const { return motor_angle_out;} // 0 - 4096

        /**
        * 获取电机当前速度
         * @return 返回数值为100*r/s
         */
        [[nodiscard]] float get_speed() {
            const float a = filter(motor_speed) * 100.0f;
            return a;
        }

        /**
         * 电机速度闭环设置
         * @param rate 电机速度，支持正反转
         * @attention 需要特别注意转速测量方向与运动方向的极性一致
         */
        void Set_Motor_Rate(const float rate)
        {
            const auto temp = static_cast<int16_t>(rate_pid(motor_speed,rate));
            Set_Motor(temp);
        }

        /**
         * 复位电机状态，同时复位速度环状态
         */
        void reset()
        {
            Set_Motor(0);
            rate_pid.reset();
        }

        void set_coder(const float _coder){one_circle_pause = _coder * 4.0f;}
        PI_Controller rate_pid = PI_Controller(100.0f,80.0f,960.0f);
        Kalman_FristOrder filter = Kalman_FristOrder(1,50);
    private:
        GPIO_TypeDef* Break_Port = nullptr;
        uint16_t Break_Pin = 0;
        GPIO_TypeDef* Phase_Port = nullptr;
        uint16_t Phase_Pin = 0;
        TIM_HandleTypeDef* _htim = nullptr; // pwm生成
        uint32_t tim_Channel = 0;
        TIM_HandleTypeDef* _htim_coder = nullptr; // 编码器
        int16_t last_coder = 0;
        float motor_position_angle = 0; // 位置环位置记录
        float motor_speed = 0.0f; // 速度计算
        float motor_angle = 0.0f;
        float one_circle_pause = 0.0f; //  电机单圈脉冲数
        float motor_angle_out;
        uint8_t speed_times = 0;
    };
} // namespace Driver_DC_Motor
#endif /* MOTOR_DRIVER_USER_INC_DRIVER_H_ */
/**
 * @}
 */

/**
 * @}
 */

/**
 * @}
 */
/*********************************END OF FILE*********************************/
